

uint8_t Data_OpenMVBuf[8];
char angle=0;

void OpenMV_Stop(void)
{
    DCMotor.Stop();
}

void OpenMV_Go(uint8_t speed, uint16_t dis)
{
    DCMotor.Go(speed, dis);
}

// void OpenMV_Track(uint8_t Car_Speed)
// {
//     uint32_t num = 0;
//     Car_Speed = 50;
//     // 清空串口缓存
//     while(ExtSRAMInterface.ExMem_Read(0x6038) != 0x00) ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 1); 
//     OpenMVTrack_Disc_StartUp();
//     delay(500);
//     DCMotor.SpeedCtr(Car_Speed, Car_Speed);
//     while(1)
//     {
//         if(ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)               //检测OpenMV识别结果
//         {   
//             ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 8);        
//             if((Data_OpenMVBuf[0] == 0x55) && (Data_OpenMVBuf[1] == 0x02) && (Data_OpenMVBuf[2] == 0x91)) 
//             {
//                 num++;
//                 Serial.println(num);
//                 if(Data_OpenMVBuf[4] == 1)  // 路口
//                 {
//                     OpenMV_Stop();
//                     OpenMVTrack_Disc_CloseUp();
//                     break;
//                 }
//                 else  // 调整
//                 {
//                     // if(Data_OpenMVBuf[3] == 1)
//                     // {
//                     //     DCMotor.SpeedCtr(Car_Speed, Car_Speed);
//                     // }      
//                     if(Data_OpenMVBuf[6] <= 5)     // 车身正无需校准
//                     {
//                         if(Data_OpenMVBuf[5] == 43)     // +  向左调
//                         {
//                             DCMotor.SpeedCtr(Car_Speed, Car_Speed);
//                         }
//                         else if(Data_OpenMVBuf[5] == 45)     // -  向右调
//                         {
//                             DCMotor.SpeedCtr(Car_Speed, Car_Speed);
//                         }
//                     }
//                     else if(Data_OpenMVBuf[6] <= 15)    // 车身微偏
//                     {
//                         if(Data_OpenMVBuf[5] == 43)     // +  向左调
//                         {
//                             DCMotor.SpeedCtr(20, 50);
//                         }
//                         else if(Data_OpenMVBuf[5] == 45)     // -  向右调
//                         {
//                             DCMotor.SpeedCtr(50, 20);
//                         }
//                     }
//                     else if(Data_OpenMVBuf[6] <= 25)
//                     {
                        
//                         if(Data_OpenMVBuf[5] == 43)     // +  向左调
//                         {
//                             DCMotor.SpeedCtr(10, 50);
//                         }
//                         else if(Data_OpenMVBuf[5] == 45)     // -  向右调
//                         {
//                             DCMotor.SpeedCtr(50, 10);
//                         } 
//                     }
//                     else if(Data_OpenMVBuf[6] <= 35)
//                     {
                        
//                         if(Data_OpenMVBuf[5] == 43)     // +  向左调
//                         {
//                             DCMotor.SpeedCtr(5, 50);
//                         }
//                         else if(Data_OpenMVBuf[5] == 45)     // -  向右调
//                         {
//                             DCMotor.SpeedCtr(50, 5);
//                         } 
//                     }
//                     else if(Data_OpenMVBuf[6] <= 50)
//                     {
                        
//                         if(Data_OpenMVBuf[5] == 43)     // +  向左调
//                         {
//                             DCMotor.SpeedCtr(0, 40);
//                         }
//                         else if(Data_OpenMVBuf[5] == 45)     // -  向右调
//                         {
//                             DCMotor.SpeedCtr(40, 0);
//                         } 
//                     }
//                     else if(Data_OpenMVBuf[6] <= 70)
//                     {
                        
//                         if(Data_OpenMVBuf[5] == 43)     // +  向左调
//                         {
//                             DCMotor.SpeedCtr(-5, 30);
//                         }
//                         else if(Data_OpenMVBuf[5] == 45)     // -  向右调
//                         {
//                             DCMotor.SpeedCtr(30, -5);
//                         } 
//                     }
//                     else if(Data_OpenMVBuf[6] > 70)
//                     {   
                        
//                         if(Data_OpenMVBuf[5] == 43)     // +  向左调
//                         {
//                             DCMotor.SpeedCtr(-10, 20);
//                         }
//                         else if(Data_OpenMVBuf[5] == 45)     // -  向右调
//                         {
//                             DCMotor.SpeedCtr(20, -10);
//                         } 
//                     }
//                 }
//             }
//         }
//     }
// }

//Android通过主车设置舵机角度

void Servoangle_control(uint8_t data)
{
   if(data == 0x2D)                       //判断副指令第二位为上升还是下降     
   {
      angle = angle-5;                    //在原始数据减5
      if(angle < -80)angle = -80;         //减到-80，直接控制在-80
   
   }else if(data == 0x2B)              
   {
      angle = angle+5;  
      if(angle > 20)angle = 20;
   }else if(data == 0x2C)
   {
      angle = 0; 
   }
   delay(100); 
   Servo_Control(angle);     
}
